#!/bin/bash

# 里程计校准脚本

echo "==================== 里程计校准系统 ===================="

# 设置环境变量
export LANG=zh_CN.UTF-8
export LC_ALL=zh_CN.UTF-8

# 检查ROS环境
if [ -z "$ROS_DISTRO" ]; then
    echo "错误: ROS环境未设置"
    echo "请运行: source /opt/ros/melodic/setup.bash"
    exit 1
fi

echo "ROS环境: $ROS_DISTRO"

# 检查catkin工作空间
if [ -z "$CATKIN_WS" ]; then
    echo "使用默认catkin工作空间: ~/catkin_ws"
    CATKIN_WS="$HOME/catkin_ws"
fi

if [ ! -d "$CATKIN_WS" ]; then
    echo "错误: catkin工作空间不存在: $CATKIN_WS"
    exit 1
fi

# 进入工作空间并source环境
cd $CATKIN_WS
source devel/setup.bash

echo "工作空间: $CATKIN_WS"
echo ""

# 检查roscore
if ! pgrep -x "roscore" > /dev/null; then
    echo "启动roscore..."
    roscore &
    sleep 3
fi

echo "roscore状态: ✓ 运行中"
echo ""

# 检查底盘是否运行
if ! rosnode list | grep -q "scout_base"; then
    echo "警告: 底盘节点未运行"
    echo "请先启动底盘: roslaunch scout_base scout_mini_base.launch"
    echo ""
fi

# 检查里程计话题
if ! rostopic list | grep -q "/odom"; then
    echo "错误: 未找到里程计话题 /odom"
    echo "请确保底盘节点已启动"
    exit 1
fi

echo "里程计话题状态: ✓ 已找到"
echo ""

# 启动校准节点
echo "启动里程计校准节点..."
echo "请按照程序提示进行操作"
echo ""

roslaunch scout_sensor_monitor odometry_calibration.launch
